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ABSTRACT 



Kalman filtering techniques are applied to a two sensor bearings only passive 
target motion analysis problem. An algorithm is developed to simulate tracking long 
range maneuvering airborne targets. The target tracking performance of the filter is 
evaluated using computer generated noisy bearing measurements. The performance of 
the filter is satisfactory given reasonable initial conditions and measurement noise. 
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THESIS DISCLAIMER 



^ ^ £ 

■yi Cr '' 

M 



The reader is cautioned that computer programs developed in this research may 
not have been exercised for all cases of interest. While every effort has been made, 
within the time available, to ensure that the programs are free of computational and 
logic errors, they cannot be considered validated. Any application of these programs 
without additional verification is at the risk of the user. 
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I. INTRODUCTION 



Air defense of a carrier battle group is becoming significantly more complex due 
not only to the increased speed and range of potentially hostile aircraft but also to 
more capable enemy targeting systems and greater cruise missile ranges. To reduce the 
probability of an aircraft carrier being successfully targeted by an enemy cruise missile 
earning aircraft, it is imperative that fighter intercept be accomplished beyond the 
maximum range of the cruise missile. Long range over-the-horizon (OTH) target 
detection and tracking are necessary to achieve this goal. 

A major obstacle common to all air defense scenarios is the enemy's use of 
electronic countermeasures (ECM). Attacking enemy aircraft will undoubtedly employ 
jamming as well as other forms of ECM to degrade or deny effective tracking by active 
systems. Therefore, the ability to passively track is required in order to successfully 
engage attacking aircraft in a dense ECM environment. 

One viable approach to this problem is passive Target Motion Analysis (TMA). 
The purpose of TMA is to determine the target's position, course and speed through a 
series of passive noisy measurements. For the air defense scenario, these passive 
measurements may be lines of bearing (LOB) obtained from the enemy aircraft's 
jamming strobes or from the electromagnetic radiation of the aircraft's long range 
targeting radar. 

Passive bearings only TMA may be performed by one or more sensors. The two 
primary considerations in evaluating TMA performance are solution accuracy and 
timeliness. Single sensor TMA requires that the observer aircraft perform zig_zag 
maneuvers to establish a target bearing rate so that the range to the target may be 
estimated. One drawback to single sensor TMA is the fact that these maneuvers may 
detract from the observer aircraft's primary mission. Also, a reasonable initial e stima te 
of the target's state (position, course and speed; is necessary to ensuie that the tracking 
solution converges in a timely manner, if indeed it converges at all. An inherent 
difficulty with bearings only TMA by a single sensor is that the solution accuracy and 
timeliness re ly quite hea vily upon a "good" a priori estimate of target range. 
Consequently, in a long range tracking scenario where the range to the target may 
exceed several hundred miles, accurate tracking by a single sensor using only bearin g 
observations is extremely arduous and rather impractical. 
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A practical solution to long range OTH passive tracking is multi-sensei, 
triangulation. High speed air targets can be accurately tracked by two or more highly 
directional sensors that are spaced sufficiently far apart. The primary reason that multi- 
sensor tracking is superior to single sensor TMA is that estimates of target range are 
continually being generated through triangulation of sensor bearing lines. Multi-sensor 
tracking is far less dependent on accurate a priori state estimates than is single sensor 
tracking for timely convergence. The major obstacle, however, in using the multi-sensor 
triangulation method is a practical one: very close cooperation is required between the 
observers in order to achieve an accurate tracking solution. Three ingredients are 
required to localize a target: the position of each sensor, the time of the observation, 
and the bearing measurement from each sensor. Ideally, all observations would be 
performed synchronously. If asynchronous lines of bearing are encountered, then 
computer processing is required to interpolate these LOB's to produce "synchronous" 
measurements. The observers are, in effect, remote sensors that transmit noisy bearing 
data to a central processing platform where the actual target tracking is performed. For 
tracking a long range and rapidly closing air target, triangulation provides a 
significantly more accurate and timely tracking solution. 

Because each sensor generates its own sequence of noisy bearing observations, 
the Kalman filter is ideally suited for determining a target's position and motion. This 
thesis investigates the two sensor bearings only tracking problem in a computer 
simulation that employs Kalman filtering techniques. The simulation generates the 
target and observer tracks as well as noisy bearing measurements from each sensor to 
the target. The noisy bearings are then processed by the Kalman filter to provide 
continual estimates of the target's state. The tracking algorithm is used in several 
scenarios to determine the effect various sensor bearing accuracies and initial estimates 
have on the filter's performance. 

The aim of this research is to examine how well the filter performs in tracking a 
non-maneuvering target before investigating the more difficult case of a maneuvering 
target. The problem geometry wiii be presented first, followed by the development of 
the system and measurement models. Relevant equations from Kalman filtering theory' 
will then be briefly reviewed before the actual tracking algorithm is analyzed in detail. 
The results of several simulation runs using various parameters will be examined. Also, 
the effect of target maneuvers on filter stability will be assessed. The final chapter will 
summarize the results of this research and will present conclusions and 
recommendations for further study. 
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II. PROBLEM DESCRIPTION 



A. INTRODUCTION 

As the name implies, over-the-horizon detection and tracking means positioning 
sensors out near the radar horizon to look over the edge and pass their observations 
back to a central data fusion point for analysis. This data fusion point need not be a 
surface combatant; it could be a command and control aircraft. The use of an airborne 
command and control platform extends the range to which an air target can be 
effectively tracked. The basic idea behind OTH tracking is to place the remote sensors 
far enough apart so that effective triangulation fixes may be taken but not so far apart 
that they are beyond the range at which they can communicate with the central 
processing platform. The command and control aircraft should ideally be positioned on 
the threat axis between the incoming air attack and the high value unit (HVU) that is 
being protected. Figure 2.1 depicts the general geometry of a basic OTH detection and 
tracking scenario. 
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Figure 2.1 Basic Over-the-Horizon Detection and Tracking Scenario. 

In this chapter the geometry of the two sensor TMA problem will be presented 
along with a development of the target motion and noise-free measurement equations. 
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B. PROBLEM GEOMETRY 



Consider the target-observer geometry in the two dimensional plane as shown in 




Figure 2.2. The target is located at (xy , >y) from a defined reference position. The 
origin may be defined as either a fixed latitude/longitude coordinate or the position of 
a high value unit such as an aircraft carrier (whose position is relatively stationary). 
The x and y components of target velocity are denoted as Xy and >y and are the 
Cartesian equivalents of the target's course and speed, Cy and Vy. Sensor 1 is located 
at (Xj . 0) and is only able to move along the x-axis with velocity x Likewise, sensor 2 
is located at (0 , y,) and is only able to move along the y-axis with velocity v,. As 
chov.T! in Fienre 2.2, the be?. rin e from 1 to the t?rec r 0 ?nd the be?rin a from 

sensor 2 to the target is 0 2 . The ranges to the target from sensors 1 and 2 are denoted 
as r, and r, respectively, and the range of the target from the origin is denoted Ry. 

1. Problem Assumptions 

The following assumptions are made concerning the problem: 

1. The target is initially inbound and remains within the first quadrant. 

2. The target maintains a constant speed but is free to change course. 
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3. Both sensor positions are known precisely. 

4. Bearing data from each sensor are continuously observed and are received 
synchronously. 

5. Bearing noise is zero mean and Gaussian with variances G 2 and c 2 2 for sensors 
1 and 2, respectively. 

6. Target turns are modeled as instantaneous (i.e., no turn radius). 

7. Target and sensor altitudes have negligible effect at long ranges. 

The last assumption is entirely reasonable since the difference between the target's 
slant range and two dimensional range is less than a fraction of 1% for ranges 
exceeding 300 nautical miles. 

2. Practical Geometrical Considerations 

Placing the airborne sensors on orthogonal axes is chosen not only because it 
simplifies the geometry but also because it provides adequate sensor separation with 
which to perform accurate triangulation. Ideally, the most accurate triangulation fix is 
formed from the intersection of two perpendicular lines of bearing. Perpendicular 
LOB's in real world scenarios, however, are extremely difficult to obtain for a number 
of reasons. One reason is that the maximum range and on station time for airborne 
sensors are limited. Also, if electromagnetic energy from the target is being used to 
obtain LOB's, it is important that both sensors be positioned within the main sector 
beam pattern. Figure 2.3 illustrates some of these considerations. In Figure 2.3 (a), it 
can be seen that in the attempt to obtain perpendicular LOB's to the target, sensor 1 is 
beyond the radar horizon of the command and control aircraft and is thus unable to 
pass any bearing observations. Figure 2.3 (b) shows both sensors lying within the 
sector scan limits of the target's surveillance radar. While it is not necessary for both 
sensors to be within the main beam simultaneously, both must be able to detect the 
beam's presence within a reasonable time period, a factor which depends on the radar's 
scan rate. 

The scenario where the sensors are positioned on orthogonal axes could easily 
be modified to the more general case where the sensors are 'located on radials that are 
separated, say, by 60 degrees. Since the sensors are now closer together, range 
estimates to the target would be somewhat degraded. Also, by adding a third sensor on 
a radial 120 degrees from the first sensor and 60 degrees from the second would 
provide a wider sector coverage as well as improved tracking accuracy. 

The simulations that have been run in this thesis involve extreme ranges from 
each sensor to the target. It has been assumed throughout that the target and sensor 
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to (L) 



Figure 2.3 Practical Geometric Considerations. 

aircraft are flying at medium to high altitudes so that all observations will meet radar 
horizon range constraints. It should be noted, however, that the practical limiting 
factors for maximum detection range are the strength and radio frequency (RF) of the 
intercepted source signal. Also, bearing accuracies depend on the RF of the signal. 
Extreme detection ranges, sometimes between 400 and 500 nautical miles, have been 
used to represent a worst case tracking problem; shorter ranges would yield a more 
accurate tracking solution. 

C. SYSTEM MODEL 

As shown in Figure 2.2, lines of bearing from two airborne sensors are used to 
determine the target's state (position, course and speed). Using a Cartesian coordinate 
system, a four dimensional state vector, x-p, is chosen. 

(eqn 2.1) 




It should be noted that the system model is in no way limited to a Cartesian reference 
frame or state vector; the Cartesian coordinate system was chosen merely for its 
mathematical simplicity. 
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1. Target Maneuvers 

Two basic scenarios are addressed in this thesis. The first one involves 
tracking a non-maneuvering target and the second involves a maneuvering target. In 
both cases the target is assumed to be initially inbound and any target maneuver will 
consist of the target changing only its course and maintaining its speed. Figure 2.4 
shows the target tracks that will be examined in subsequent chapters. 




Figure 2.4 Representative Target Tracks. 

It is assumed that target maneuvers can be modeled by using white random 
forcing functions. As shown in Figure 2.5, target maneuvers may be thought of as 
acceleration along its course (radial acceleration) and acceleration perpendicular to its 
course (turn rate). Let the random variables 8* and 8^ denote the target's acceleration 
along its course and acceleration perpendicular to its course, respectively. Both 8. and 
8^ denote random changes of the target and are assumed to be independent and zero 
mean with variances a. 2 and tr^ 2 . Because of the extremely long ranges involved in the 
simulations, target maneuvers have been modeled as instantaneous changes of position 
according to the time interval used. That is, the simulation enables the target to turn 
90° in two seconds. While it is acknowledged that this kind of turn rate is quite 
artificial, it is informative to see what effect such a drastic turn rate has on tracking 
performance and stability. The variances used in subsequent scenarios are: 
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(eqn 2.2) 



<r. 2 = (300knots/sec) 2 



ffg 2 = (45 deg,' sec) 2 



(eqn 2.3) 




Figure 2.5 Geometry of Target Maneuvers. 
a. Equations of Motion 

Let T represent the time interval between observations. If k represents the 
k* observation and t^ the discrete time of the k* observation, then T may be 
expressed as 



T “ l k r k-l 



(eqn 2.4) 



Referring to Figure 2.2 and Figure 2.5, target motion may be described by the 
difference equations: 






x T (k+l)" 




"x T (k) + T x T (k) + fj(5 v , 6 0 , k) 


x T (k+ 1) 




x T (k) + f 2 (6 v , 6 0 , k) 


y T (k+ 1) 




y T (k) + T y T (k) + f 3 (S v , 6 0 , k) 


y T (k+D 




yj(k) + r 4 (6 v , 6 0 , k) 



(eqn 2.5) 
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The random forcing functions fj through f 4 are included to account for random 
changes in speed and heading which occur for a moving target. Equation 2.5 may be 
written in matrix form as 





1 T 0 0 




Xy(k) 




T 2 /2 0 




0 10 0 




Xy( k) 




T 0 


x(k+ 1) = 


0 0 1 T 




yj( k ) 




0 T 2 /2 




0 0 0 1 




yj( k ) 




< o 

H 

L 



x T (k) 

yj(k) 



(eqn 2.6) 



or more concisely as 



x k+l = °k x k + r k”k 



(eqn 2.7) 



where is the 4 x 1 state vector 

Oj. is the 4 x 4state transition matrix 

Wj. is the 2 x l vector of random forcing functions 

r j. is the 4 x 2 state forcing matrix. 

The terms of the random forcing function vector represent the 
accelerations in the x and y directions caused by target maneuvers. The state forcing 
matrix T^. represents a uniform constant acceleration model of target motion. If the 
time interval T between measurements is assumed constant, then may be replaced 
by a constant state transition matrix <P and 1"^ may be replaced by a constant state 
forcing matrix T. Revising Equation 2.7, the linear system model can be expressed as 



x k+l = ^ x k + *~ w k (eqn 2.8) 

D. NOISE-FREE MEASUREMENT EQUATION 

As illustrated in Figure 2.2. the positions of sensors 1 and 2 along with their 
respective bearings to the target. 0j and 0.,, uniquely define the target's position (Xy, 
Yy). The target's position from noise-free bearing observations may be expressed as 

(y 2 cos 0j - Xj sin 0 } ) sin 0 2 

^ ~ cos(0 1 + 0 2 ) (eqn 2.9) 
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(eqn 2.10) 



(Xj cos 0, - y, sin 0,) sin 0 L 
^ ~ cos(0j + 0 2 ) 

The positions and speeds for the airborne sensors may be chosen arbitrarily for 
input into the tracking algorithm. Each sensor's position is assumed to be known 
precisely for each time interval. The sensors may both head inbound or outbound or 
they may go in alternate directions. Care must be exercised in choosing sensor 
positions and speeds so as to avoid having lines of bearing that are collinear (each 
sensor is pointing at the other). What results in this case is an extremely thin and 
elongated error ellipse which momentarily degrades tracking accuracy at the moment 
that the lines of bearing are coincident. 

It should be noted that using two sensors eliminates the need for any extraneous 
observer maneuvering as is the case for a single sensor. The observer aircraft can 
basically fly straight and level and collect more reliable bearings to the target. Also, the 
sensor's position is known more precisely since it is not decelerating and accelerating 
into and out of turns. 
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III. KALMAN FILTERING 



A. INTRODUCTION 

The technique of Kalman filtering is ideally suited to the problem of passive 
tracking. The following sections briefly describe the theory and results of Kalman 
filtering and how it is applied to the long range airborne TMA problem. For a more in- 
depth development of the Kalman filter, the reader is referred to [Refs. 1,2]. 

B. THE KALMAN FILTER 

The purpose of the Kalman filter is to keep track of the state of a system 
through a sequence of noisy measurements. This is accomplished by recursively 
updating an estimate of the state by processing a sequence of noisy observations in 
such a manner as to reduce as much as possible the effect of measurement errors. 

The Kalman filter is a predictor-corrector type estimator that propagates an 
estimate, x, of the target state along with an associated covariance matrix, P, which 
reflects the degree of confidence placed in the accuracy of the state estimate. The 
Kalman filter is carried out in two alternating stages. First, previous estimates of x 
and P are extrapolated one time step ahead based on the assumed system dynamics; 
this is referred to as the Movement Step. These extrapolated values are then used to 
compute a set of optimum weights called Kalman gains. The gains are applied to the 
prediction and to a new observation in a Measurement Step, which provides an 
updated estimate of the state and its covariance. This process is then repeated. [Ref. 3] 

1. Assumptions 

The following assumptions are made: 

1. The random forcing function Wj. is zero mean and uncorrelated with covariance 

Qk- 

2. The measurement noise vj, is zero mean and is correlated with covariance Rj.. 

3. The random forcing function Wj. and measurement noise v^, are uncorrelated. 

4. The initial state x Q is a random variable with known mean x 0 |-i an ^ covariance 
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2. Definitions 



1. The estimated state vector after k observations is denoted by x k)k and the 
predicted state vector before the k 111 observation is represented by x k | k ,j. 

2. The state estimation error vector £ k is defined as the difference between the 
estimated state and the true state 



C k ~ X k|k X k 

and the predicted state estimation vector £ k k-1 
the predicted state and the true state 



(eqn 3.1) 

is defmed as the difference between 



A 

C klk-1 = X k|k-1 ” X k 

3. The covariance of estimation error matrix P k)k is defmed as 



(eqn 3.2) 



P klk c k c k"^ (eqn j.3) 

and the predicted covariance of state error matrix P k)k .j is defmed as 

P klk-i = c k!k-i c k| T k-i ) 

4. The state excitation covariance matrix is given by 

Q k = E{ r w k wj r T ) (eqn 3.5) 

5. The Kalman filter is an optimal estimator that minimizes the sum of the 
variances of the estimation error, i.e., 



E(c,(k) 2 } + E{c,(k) 2 } + . . . + E(C n (k) 2 } (eqn 3.6) 
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Enter known matrices and a priori estimates: 



%)|-I) - P (0|-l) • R (0) • H • ^ 



Compute the Kalman gain: 



'(k) " P (k|k-1) HT ' HP (k|k-l) HT + R (k) > 



•1 



MEASUREMENT STEP 

x (k|k) = X (k|k-I) + G (k)f z (k) ' Hx (k|k-1) > 
P (k|k) = n - G( k )H }P (k | k .j) 

MOVEMENT STEP 

x (k+ Ilk) = ° x (k|k) 

P (k+l|k) = ^^klk)^ 1 + Q(k) 



Compute 



and Q (k) 



Increment k by 1 



Figure 3.1 Kalman Filter Algorithm. 
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3. Kalman Filter Algorithm 

Figure 3.1 summarizes the discrete Kalman filter algorithm. For the particular 
TMA problem presented in this thesis, the 2 * 4 measurement matrix H k and the 4 * 4 
state transition matrix are both known, constant matrices and may be represented 
by H and <J>. An a priori estimate Xgjj of the target's state with an associated initial 
error covariance matrix P 0 |_|. as well as an initial estimate of the measurement noise 
covariance matrix R Q must be input into the filter algorithm. The algorithm computes 
the Kalman gain G R based on these a priori values and then updates the estimate of 
the target's state when it receives a measurement. The error covariance matrix is also 
updated. Next, the state estimate and its error covariance matrix are projected one time 
step ahead based on the assumed system dynamics. The measurement noise covariance 
R k and the state excitation covariance matrix Q k are then computed before k is 
incremented by one and the whole process is repeated. 

C. FUNCTIONS, MATRICES, AND EQUATIONS 

In this section, the Kalman filter algorithm will be applied to the long range 
passive airborne tracking problem. A brief derivation of the random forcing function 
w k> the state excitation covariance matrix Q k , the measurement equation Zj., and the 
measurement noise covariance matrix R k is given next. 

1. Random Forcing Function 

Recalling equation (2-6), the two dimensional random forcing function \v k 
represents the acceleration in the x and y directions caused by target maneuvers. 





X 




>\ 6 0 + ('Y V k )6 v 




1 

l 2 L _ 




_-\ 6 0 + (>W\ 



(eqn 3.7) 



where \' k = ( x k 2 + y k 2 ) 1 ' 2 . 

Since the random variables 5^. and 5^ were assumed to be zero mean, it 

follows that the random forcing function is also zero mean. The variances of the x 

and y accelerations, denoted by <r.. 2 and <7-- 2 respectively, are 

x y 



V “ (eqn 3.S) 

c./ -E[i t ] = i* (=qn 3.9) 
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The covariance of the x and y acceleration <y~.. 2 is 

xy 



Therefore, the random forcing function covariance matrix Q k ' is 



<V = w k T ) = 



<r 1 .. 

X x y 

oft 

*y y 



(eqn 3.10) 



(eqn 3.11) 



where , <Ty 2 , and (J-~ 2 are computed at the predicted values of Xj and yj. 

2. State Excitation Covariance Matrix 

The purpose of the state excitation covariance matrix Q k is to account for 
model inaccuracies or for a target that has maneuvered. It is basically a "procedure for 
masking the effects of modeling errors" [Ref. 2: p. 163]. In effect, the state excitation 
covariance matrix increases the size of the predicted covariance of error matrix which 
in turn increases the filter gains. As more observations are processed, Q k prevents the 
Kalman gains from approaching zero by continually injecting uncertainty into the 
predicted state estimate at each iteration. A nonzero Q k slightly degrades the filter's 
accuracy when the target is not maneuvering but it helps prevent filter divergence when 
the target dc 
matrix is 



maneuver. As stated in 


equation 


3.5, the 




XV/ T s i 


XV 2 .. 

*i «y 


LV~ 


*-) 

H 

II 


T* 1 

T <r. 


xV.. 
z X 1 

XV 


tV, 




SYmmETRlC 


y y 


L 1 

r ^ 











(eqn 3.12) 



3. Measurement Equation 

In this TMA problem, the observations are noisy (x,v) positions. It is the 
intersection of noisy lines of bearing that form the noisy (x,y) position of the target 
that is input into the Kalman filter algorithm. Because the observations are of the 
same form as the state vector, the measurement equation is linear and is expressed as 
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h = H x k + \ 



(eqn 3.13) 



where z k is the 2 * 1 measurement vector 

H is the 2 * 4 measurement matrix 
v k is the 2 * 1 measurement noise vector 
x k is the 4 x l state vector. 

The equation may be written explicitly as 
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v lk 

V 2k 



(eqn 3.14) 



The most important part of the measurement equation is an accurate description of the 
measurement noise vector v k The measurement noise vector expresses the statistical 
nature of the noisy (x,v) position that is derived from the intersection of two noisy lines 
of bearing. These bearing measurement errors are assumed to be independent and zero 
mean with variances ffj 2 and c 2 2 , for sensor 1 and sensor 2 respectively. 

It is important to note that the bearing errors between sensors are statistically 
uncorrelated; one sensor's bearing accuracy has nothing to do with any other sensor's 
bearing accuracy. However, in describing the resulting intersection in Cartesian 
coordinates, the noisy x and noisy y positions are correlated. The only case where the 
noisy x and noisy y positions are uncorrelated is when the lines of bearing are 
perpendicular. 

4. Error Ellipses 

An intuitive way to visualize the measurement equation is through the concept 
of error ellipses. Error ellipses give a geometric picture of the region around a noisy 
position or estimate where the true value is considered to lie. Figure 3.2 shows a 
bivariate Gaussian probabili Tv dcnsit v function formed b v the intersection of wo lines 
of bearing with independent Gaussian distributions. 

As can be seen in Figure 3.2, the lines of bearing intersect at an oblique angle, 
forming an asymmetric hump. While the bivariate Gaussian probability density 
function gives an interesting three dimensional depiction of two normally distributed 
bearing errors, it does not provide the information that is really needed, quickly. What 
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Bivariate Normal Density Surface: Sx=3, Sy=5, p=.5 




Figure 3.2 Bivariate Gaussian Probability Density Function. 

is needed is an accurate picture of the measurement (or estimation) error. This 
uncertainty is best expressed geometrically by the error ellipse. The term "error ellipse" 
refers to the two dimensional surface of constant probability density. Figure 3.3 
presents these error ellipses as contour lines of the bivariate Gaussian probability 
density function shown in Figure 3.2. 

The various ellipse sizes in Figure 3.3 correspond to different constant 
probabilities. The fact that the ellipses are also rotated implies that the uncertainty in 
measurement error is indeed correlated with respect to x and y. The actual probabilities 
within a specified error ellipse may be computed through lengthy integration of the 
bivariate Gaussian probability density function over the surface of the ellipse. Some 
computed probabilities of the true value lying within the Iff, 2ff, and 3ff error ellipses 
are .394, .865, and .9S9 respectively [Ref. 4: pp. 4-49). 

Error eliipses are extremely useful in examining postion error. Matrices 
containing the x and y position terms convey analytically what error ellipses display 
graphically. A 2 x 2 error covariance matrix which contains position components x and 
y is able to completely describe an ellipse. The main diagonal terms represent the 
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Contours of the Bivariate Normal Density Surface 




Figure 3.3 Error Ellipses as Contour Lines. 

variances of the x and v positions respectively. The off diagonal terms represent the 
degree of x-v coupling and the orientation of the error ellipse in the x-y plane. 

5. Covariance of Measurement Error Matrix 

The covariance of measurement error matrix R k uses the concept of error 
ellipses to accurately describe the noisiness and degree of coupling of (x,y) 
measurements obtained from intersections of noisy LOB's. The terms of the covariance 
of measurement error matrix R k depend on the standard deviations of bearing error <Tj 
and C 2 of sensors 1 and 2 as well as the angle at which the lines of bearing intercept. 
The covariance of measurement error may be expressed as 



R = 



S ir?9 z + co* 6 , -0^ *i#N0 1 cosfy — dj? co$6, s« 



co £(e t *e^ 
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where 0j and 0 ; denote noisy bearing observations from sensors 1 and 2, respectively. 
The subscript k has been intentionally deleted from equation (3-15) only for ease of 
notation. At each discrete time interval t k , new values of 0j and 0, are generated with 
which to compute the new measurement error covariance matrix, R k A complete 
derivation of equation (3-15) is given in Appendix A for the interested reader. 
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IV. THE ALGORITHM 



A. INTRODUCTION 

This section discusses the development of the tracking algorithm. The algorithm 
is designed to simulate tracking a long range inbound enemy air target by triangulating 
noisy bearing observations from two airborne sensors. We want to be able to track a 
non-maneuvering target within a one percent range error. For a maneuvering target, 
we desire a stable filter response which quickly converges to the target's new state. The 
effect of various sensor bearing errors, a priori state estimates and initial error 
covariance matrices on filter accuracy and convergence time are investigated. Also, the 
effect of target maneuvers on filter stability is analyzed. Basically, the algorithm 
performs three functions: 

1. The target and sensor tracks are generated. 

2. Noisy bearing observations are simulated using a random number generator. 

3. The noisy measurements are processed by the Kalman filter algorithm to 
generate estimates of the target's state. 



B. TARGET TRACK 

As mentioned in Chapter 2, three target track scenarios are investigated: 
nonmaneuvering, gentle turn, and hard turn. In all three cases the initial target position 
is (410 nmi, 430 nmi) with X and Y velocities of —400 knots and - 380 knots 
respectively. 

C. GENERAL SIMULATION SCENARIO 

The overall purpose of this simulation is to be able to track an inbound enemy 
aircraft before it flies within 300 nmi of the high value unit. By using two sensors which 
are essentially abie to "peek" over the horizon, a long range OTH iignter intercept of 
the target aircraft may be accomplished. For all simulation runs, an initial target range 
of 600 nmi is chosen. This allows the sensors to passively track for thirty minutes, and 
it enables fighter intercept to occur beyond 450 nmi of the HVU, depending on the 
fighter's initial position and fuel state. The target aircraft is also assumed to be flying 
inbound at approximately 600 knots. 
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1. Algorithm Flow 

The algorithm can be broken down into the following steps: 

1. Define the true target track. 

2. Define the observer tracks. 

3. Enter a priori estimates x 0 |_j, P Q |_j, and Rq. Enter bearing error variances <Jj 2 
and c 2 2 . 

4. Compute the noise free bearings from each sensor to the target for each time 
interval. 

5. Compute random sensor bearing errors using computer generated normal 
distribution. 

6. Add the random bearing errors to the noise free bearings to create noisy 
bearing measurements. 

7. Compute the noisy (x.v) position that results from the intersection of two noisy 
LOB's. 

S. Input this noisy (x,y) measurement into the Kalman filter algorithm. 

D. TARGET TRACK 

As mentioned in Chapter 2, three target track scenarios are investigated: 
nonmaneuvering, gentle turn, and hard turn. For all scenarios, the initial target 
position is (410 nmi, 430 nmi) with x and v velocities of —400 knots and — 380 knots 
respectively. For the scenarios where the target maneuvers, a value is input for the time 
that the maneuver is to take place. Also, values for the x and y velocities are input for 
the second leg of the target track. It should be noted that the turn radius of the target 
is not taken into account in the target track, for at the extreme distances being 
investigated, target maneuvers almost appear as point turns. 



E. OBSERVER TRACKS 



The observer tracks each begin at 295 nmi on their respective axis and travel 
inbound at 420 knots. These observer tracks were chosen to be inbound to represent a 
more realistic, worst case type scenario. Ideally, it is desired to have both observer 

f'l/'Vc r-n-nn rvi i t r\ i i tv H m f o olnpnct nomnnrlirulnr T f') C A Uq ^ 



constraints and maximum on-station time for the observer aircraft are important 
practical considerations that must be taken into account. 



F. INITIALIZATION 

In the first series of simulations, different combinations of the initial state 
estimate x 0 |_j and the initial error covariance matrix P 0 |.i are tested. For the first 



29 



simulation, and the one by which the other simulations are compared, the a priori state 
estimate is 



400 nmi 
420 knots 
400 nmi 
420 knots 



and its associated initial error covariance matrix is 




(50nmi) 2 

0 

0 

0 



0 

(50kts) 2 

0 

0 



0 0 

0 0 

I50nmi) 2 0 

0 (50kts) 2 



The initial measurement error covariance matrix for the one degree bearing error case 
is 



(7nmi) 2 
(5nmi) 2 

G. NOISY BEARING GENERATION 

The Box-M tiller method is used to generate normally distributed bearing errors. 
Basically, it is a mapping technique that uses an algebraic identity to establish a one to 
one relationship between a uniform random variable and a normal random variable. 
Two random U(0,1) numbers, Uj and L' 2 >are transformed into independent N(0,1) 
random numbers, N t and N, using the equations 

Nj = ( - 21nUj) 1,2 cos 2 ttU 2 

N 2 = (-21nL ,) 1/2 sin 2nU 2 




(5nmi) 2 

(7nmi) 2 
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HISTOGRAM, SSZ-901 




Figure 4.1 Normally Distributed Bearing Error. 

Figure 4.1 presents a histogram showing the normal distribution of the bearing error. 
These normally distributed random numbers are then multiplied by the standard 
deviation of measurement error for each sensor to produce two independent normally 
distributed bearing errors for 0j and 0,. 
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V. SIMULATION RESULTS 



A. INTRODUCTION 

The purpose of this chapter is to show through various scenarios the effect of 
different initial state estimates and measurement noise levels on the stability, accuracy, 
and convergence time of the algorithm. In the following pages, fourteen simulations 
involving three scenarios are presented. As shown in Figure 2.4, the three scenarios 

A 

include a nonmaneuvering target track, a target track with^gentle turn, and a track with 
a hard turn. The first scenario provides the reference with which other cases may be 
compared. Unless otherwise noted, all simulations use a two second time interval 
between measurements. Also, all of the simulation results depict the cases of one 
degree and five degree sensor bearing errors. It should be pointed out that in order to 
isolate the effect of various parameter changes, a single random number seed is used 
throughout to represent a specific noise history. There has been no attempt to create 
statistics based on ensemble of noise histories due to the extreme computational time 
required. 

B. TYPES OF GRAPHS 

Five graphs are used in all simulations. These include the x and y positional 
errors, the x and y velocity errors, and the percent range error. For the case of 
positional errors, the updated state estimate of x and v position is subtracted from the 
target's true x and y position. Likewise, for the case of velocity errors, the updated 
state estimate of velocity is subtracted from the target's true x and y velocity. Range 
percent error is computed as 

Ir -r / 

Range Percent Error = — ^ — — x 100 

“t 

where Ry denotes the target's true range from the origin and Ry is the updated 
estimate of target range using the updated state estimate for the x and y positions. In 
some simulations, the measurement residual error is plotted. The measurement residual 
is defi.;.-d as the difference between the actual noisy measurement and the predicted 
state estimate. It may be expressed as the quantity 

*k " H \|k-l 
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C. SCENARIO 1 



Scenario 1 consists of ten simulations that demonstrate the effect of various a 
priori estimates, measurement noise levels, and time intervals between measurements 
on filter performance. In all ten simulations, the target is on constant course and 
speed. Figure 5.1 illustrates the target's true track along with noisy measurements. 
Note by the scale that only a portion of the first quadrant is depicted. In the fust 
simulation, the initial difference between the true target state and the initial state 
estimate is 



— T« 




10 nmi 
20 kt 
30 nmi 
40 kt 



and the a priori error covariance matrix is 

O o o 
o (SOkif O i O 
O O (5 OnJ o 

o O o (sokif 

Overall, it can be seen that for the one degree bearing error case, the algorithm 
tracks quite well. Referring to Figure 5.1, the x velocity error initially gets worse before 
it gets better. It is not until after five minutes have elapsed that the x velocity error is 
less than the a priori estimate. As can be seen from Figure 5.1, the tracking accuracy 
for the five degree bearing error case is fair. The one degree bearing error case 
definitely demonstrates quick and accurate filter convergence: the five degree bearing 
case seems to meander almost randomly. The sudden rise for the five degree case seems 
to be an anomalous disturbance. The x and y velocity gains are directly related to 
bearing accuracy; the higher the accuracy, the greater the gain. For the five degree 
bearing error case, the velocity gain never exceeds 0.2. 

The following nine cases are basically variations of the first case. Table 1 lists 
the parameters that have been changed for each case. 
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TRUE X - FILTERED X POSmON VS. TIME 



TRUE Y - FILTERED Y POSITION VS. TIME 





TRUE Y - FILTERED Y VELOCITY VS. TIME 




PERCENT RANGE ERROR VS. TIME 




Figure 5.1 Straight Track Reference Case. 
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Figure 5.2 Straight Track Reference Case (Cont'd). 
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Figure 5.3 Time Interval = 5 sec. 
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Figure 5.4 APXHAT = 430 -540 120 -50, with R Matrix. 
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X VELOCITY ERROR (in KT) X ERROR (In NU) 

-60 -40 -20 0 0 5 10 tS 



TRUE X - FILTERED X POSITION VS. TIME 



TRUE Y - FILTERED Y POSITION VS. TIME 





TRUE X - FILTERED X VELOCITY VS. TIME 



0 10 20 30 0 to 20 30 





TRUE Y - FILTERED Y VELOCITY VS. TIME 




Figure 5.5 Same as Case 3, Uncorrelated R Matrix. 
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TRUE X - FILTERED X POSmON VS. TIME 



TRUE Y - FILTERED Y POSITION VS. TIME 





TRUE X - FILTERED X VELOCITY VS. TIME 



TRUE Y - FILTERED Y VELOCITY VS. TIME 




Figure 5.6 APXHAT = 400 -150 400 -500. 
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TRUE X - FILTERED X POSITION VS. TIME 



TRUE Y - FILTERED Y POSITION VS. TIME 





TRUE X - FILTERED X VELOCITY VS. TIME 



TRUE Y - FILTERED Y VELOCITY VS. TIME 





PERCENT RANGE ERROR VS, TIME 



X AND Y VELOCITY GAINS VS. TIME 





Figure 5.7 APXHAT = 200 -420 4S0 -420. 
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TRUE X - FILTERED X POSITION VS. TIME 



TRUE Y - FILTERED Y POSITION VS. TIME 




TIME (in minuter) 




TRUE X - FILTERED X VELOCITY VS, TIME 




TRUE Y - FILTERED Y VELOCITY VS, TIME 




PERCENT RANGE ERROR VS, TIME 



X AND Y VELOCITY GAINS VS. TIME 





Figure 5.8 APXHAT = 200 -150 480 -500 P= 10 5 I. 
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TRUE X - FILTERED X POSITION VS. TIME 



TRUE Y - FILTERED Y POSITION VS. TIME 





TRUE X - FILTERED X VElOCY VS. TIME TRUE v - FILTERED Y VELCCFY VS. TIME 






Figure 5.9 APXHAT 



200 -150 480 -500 P= 25001. 
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|<Af wl IKKOU (in pen t*nl) X VUOCITY UiftOM (m Kl) 

l) r j 11 ) 1 !» 2 0 5 0 5 10 



TRUE X - FILTERED X POSITION VS. TIME 




TRUE Y - FILTERED Y POSITION VS. TIME 




I 



TRUE X - ALTERED X VELOCITY VS. TIME 



TRUE Y - FILTERED Y VELOCITY VS. TIME 





PERCENT RANGE ERROR VS. TIME 



X AND Y VELOCITY GAINS VS. TIME 




Figure 5.10 APXHAT « 400 -410 420 -370 P= 1001. 
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TRUE X - PITER ED X POSITION VS. TIME 



TRUE Y - FILTERED Y PCS.TION VS TIME 





"RUE X - FILTERED X VELOCITY VS. T,ME TRUE Y - FIL T ERED Y VELOCITY VS TIME 





PERCENT RANGE ERROR VS. TIME 
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X AND Y VELOCITY GAINS VS. TIME 




Figure 5.11 ffj = 5 deg a 2 ~ 1 deg. 
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TRUE X - FILTERED X POSITION VS. TIME 



TRUE Y - FILTERED Y POSITION VS TIME 





TRUE X - FILTERED X VELOCITY VS. TIME 




TRUE Y - FILTERED Y VELOCITY VS. TIME 
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PERCENT RANGE ERRCR VS. TIME 



X AND Y VELOCITY GAINS VS. TIME 





Figure 5.12 Gentle Turn With Q Matrix. 
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TRUE Y - FILTERED Y POSITION VS. TIME 



TRUE X - FILTERED X POSITION VS. TIME 





TRUE X - FILTERED X VELCC ,T Y VS. T ME 




TRUE v - FILTERED v VELCC’TY VS TIME 




PERCENT RANCE ERROR VS. TIME X AND Y VELOCITY GAINS VS. TIME 





Figure 5.13 Hard Turn With Q Matrix. 
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TRUE X - FILTERED X POSITION VS. TiME 



TRUE Y - FILTERED Y POSITION VS TIME 





TRUE X - FILTERED X VELOCITY VS. TIME 




TRUE Y - FILTERED v VELOCITY VS. TIME 




TIME (m minutes) 



PERCENT RANGE ERROR VS TIME x AN & Y VELOCITY GAINS VS. TIME 




Figure 5.14 Case 11 For 45 min. run. 
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VI. CONCLUSIONS AND RECOMMENDATIONS 



A. CONCLUSIONS 

The purpose of this thesis was to investigate the two sensor bearings only passive 
tracking problem using Kalman filtering techniques. A computer simulation was 
developed to generate the target track and the noisy bearing observations from each 
sensor. Filter performance was exceptional for the nonmaneuvering target case. With 
one degree sensor bearing error and two second measurement intervals, the filter was 
able to consistently track the target to within one quarter of one percent range error in 
the first five minutes. As was expected, filter accuracy was degraded as bearing error 
was increased. The tracker performed reasonably well for sensor bearing errors as high 
as eight degrees. 

For the case of a maneuvering target, filter performance was marginal. Filter 
convergence to an accuracy attained prior to the target maneuver did not occur. The 
use of a state excitation covariance matrix by itself was not sufficient enough to 
properly account for target maneuvers. What is needed is a reliable zig detector that 
quickly recognizes target maneuvers so that the filter gains may be reinitialized. The 
probiem of detecting target maneuvers is not trivial. At long ranges, error ellipses may 
be twenty to forty times larger than the actual distance the target has moved. Sifting 
out a bona fide target maneuver from these extremely noisy measurements is quite 
difficult. Determining a target maneuver by attempting to find a pattern in the 
measurement residuals is only successful if the time between measurements is greater 
than thirty seconds. A drawback to this approach is that filter accuracy is degraded 
because fewer measurements are being processed. 

From the simulation runs it was found that the most influential factors in 
determining tracking accuracy and speed of convergence were the sensor bearing 
accuracies and the time between measurements. Factors that contributed to a lesser 
extent included the accuracy of the initial state estimate along with its associated 
degree of confidence and the positions of the sensors. Inaccurate a priori information 
did not degrade the filter's accuracy, it only increased the time for convergence. Filter 
accuracy improved as the lines of bearing came closer to being perpendicular. 
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B. RECOMMENDATIONS 



This study is by no means complete. Some areas for further study include the 
following: 

1. Run an entire ensemble of simulations (perhaps 1000 runs) to generate reliable 
statistics on the filter's performance. 

2. Investigate more fully a method to detect target maneuvers so that adaptive 
control techniques may be used to alleviate the problem of filter divergence. 

?. Examine the utility of using more sensors to cover a comparable sector. Are 
more sensors necessarily beneficial? 

4. Perform the simulation using a different coordinate system (such as polar 
coordinates) and compare the results to those obtained using a Cartesian 
model. 
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APPENDIX A 

DERIVATION OF MEASUREMENT ERROR COVARIANCE MATRIX 



In this appendix the measurement error covariance matrix R k is derived. 
Recalling the measurement equation from Chapter 3. the measurement noise vector v k 
is assumed to be Gaussian and zero mean with variance R k . That is, 



\ ~ N (°- R k> 



(A-i) 



The purpose of R k is to statistically describe the noisiness of the x and y measurements 
obtained through the intersection of noisy lines of bearing. Basically, the 2 by 2 
measurement error covariance matrix describes this noisiness by displaying the variance 
and covariance of the noisy x and v measurements in terms of each sensor's bearing 
measurement and accuracy. Referring to Figure A.l, the position (Xy.Yy) represents 
a possible true position of the target based on noisy sensor bearing observations, 0j 
and 0,. The position of the target (Xy, Yy) is a jointly distributed random variable 
whose expected value coincides with the intersection of the bearing lines. 




Figure A.l Target Observer Geometry' Revisited. 
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To develop the relationship between Xy and Yy. the LOB s 
may be expressed in the general form for the equation of a line: 


from each sensor 


X Si' nff, * y cos — X, S inP, - O 


(A-2) 


X c oi& x *■ y Sin 6 t - & x - O 


M) 


The distance from the position (Xy, Yy) to each sensor line of bearing is denoted by 
dj and d., for each sensor LOB respectively. From the problem geometry and by using 
the equation for the distance between a point and a line, the displacement distances dj 
and d, may be expressed as 


<J t = - *,) 5 >n 0, * Yy 


(AS) 


4 = r[x T co«e z *(r T - 


(A-S) 



Since both sensor bearing errors are assumed to be Gaussian zero mean random 
variables with bearing variances <Jj and a,", it follows that the displacement random 
variables d^ and d-, are also zero mean Gaussian with displacement variances and 
<7,? respectively. Figure A. 2 illustrates the relationship between the displacement 
variance and the sensor bearing error variance. 




Figure A. 2 Relationship Between Bearing and Displacement Errors. 
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In this figure, the bearing error standard deviation for sensor 1, Cj, is expressed 
in degrees or radians whereas the displacement error standard deviation for sensor 1, <y, 

, is expressed in nautical miles, They are related by 



(Tj/ = r ; tan (Jj 


(A-0 


where rj is the approximate distance from sensor 1 to the target in nautical miles. 

Therefore, the displacement distances d ( and d., are normal random variables that 
may be written as 


d ( ~ X(0, cr,, 2 ) 


(A- 1) 


d 2 ~ N(0, <T,7) 


(AS) 


Having described the displacement random variables d, and d 2 from 
sensor 2 lines of bearing, equations A-4 and A- 5 may be rewritten as 


sensor 1 and 


X T S'" * Y r COS & t = X, Sin 6, * N, 


(AS) 


X T cos £> + Y t s.n Q x - y* *•>, 9 Z + N z 


(A 1-10} 



where N, = N(0, c^) and N, = N(0. G-}). Solving equations (A-9) and (A-10) for 
Xy and V T yields: 

^ cos 9 , - X, + hJ 2 COS 9 , - Nf S>n&i 

r cos (d, + 6 t ) 

*! S.n 9 , Co<S$ L - 'it S.n 6 t tmfl, » N, cox 6 £ - M 2 
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Sing; COfA, - X , S Q t 3 1 ‘rt# 2 
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A 2 * 


- 5 I n 0 J, 


A - 


COT 0, 






<**(&, 


n *5 


L°t(e,ie^ 




a 5 


6 t Cos “ 




8$ tin 8\ 






tos (6, * 


a) 






a - 


cos $2 


p _ 


-Sin &i 






<**(&,* & t ) 







Note that since Xy and Yy are linear combinations of normally distributed random 
variables, they must also be normally distributed. That is. 



Xy ~ N'( <7y^ ) 



where ^x t = ^1 

^ = A 2 2 ff l' 2 + A 3 2 a 2' 

and fly = Bj 

Oy~ = B ^ 2 (Tj 2 + B 3 2 <? 2 2 

Now, the measurement error covariance matrix R may be written as 



R = 



Var(Xy) 
Cov(Xy, Yy) 



Cov(Xy, Yy) 
Var(Yy) 



(A- 1 3) 

(A-IH) 



(A- is) 



(A-IC) 



(A-n) 



By definition, the covariance of the random variables Xy and Yy is the expected 
value of their product minus the product of their means. That is, 

Co v(Xy, Yy) = e[x t Yy] - My^My^ ( a -\ q ) 

Substituting equations (A- 11) and (A- 12) into the above equation yields 

Cov(YrX) = E [(/»,♦ A (A-») 
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By noting that Nj and N, are zero mean, the expected value of their product is the 
product of their means. That is, E^NjN^J = eJnJ e|\,J = 0. Using this fact, 
the covariance between the random variables Xy and Yy is simplified to 



Co \(Xy, ^ y) — Ay By (Tj/” 4- Aj B 3 CTyT 



(A-Zo) 



Substituting equations (A-15), (A- 16), and (A-20) into equation (A- 1 7) enables the 
measurement error covariance matrix R to be written as 



A z + A 1 A z 8 Z erf + A , 6 3 ojf 



R = 



A z B, + A , 8, <r‘ B* <rf ♦ 8 ‘ <£ 



,2 2 



Lastly, substituting the values for A, and A 3 into the above equation yields the final 
expression for the measurement error covariance matrix 



Z 2 2 

fly' S»n©£ + fl^' COST'S, 



R = 



■ Oj’, ca<8, t,', fij 

cos*-(e,*e t \ 



~ ff ». tfsd, qy co5*e t ♦ gy *i*6, 



cof(e,+e t ) 



cosY©,^) 
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APPENDIX B 

SIMULATION PROGRAM LISTING 



[ 1 ] 
*2 
[ 3 j 
[4] 

hi 

"K 
: 8 2 
'O' 

[ 10 ] 

[ 11 ] 

[ 12 ] 

[13] 

. 14 ] 

[13] 

[16] 

[17] 

[18] 

[19] 

[ 20 ] 
[ 21 ] 
. 22 ] 

[23] 

[24] 

[25] 

[26] 

[27] 

[28] 

[29] 

[30] 

[31] 

[32] 

[33] 

[34] 

[35] 

[36] 

[37] 

[38] 

[39] 

[40] 

[41] 

[42] 
t43] 
[44 J 
[45] 
[46~ 
[47 

[48] 

[49] 

[50] 

[51] 

[52] 

[53] 

[54] 

[55] 

[56] 

[57] 

[58] 

[59] 

[60] 



V KALMAN 

a THIS PROGRAM IS A TWO -SENSOR KALMAN FILTER TRACKING ALGORITHM. 

A 

f\THE FOLLOWING IS A LIST OF THE PRINCIPAL VARIABLES USED : 

a APXHAT A PRIORI STATE ESTIMATE XHAT 

fiBEl ,BE 2 NORMALLY DISTRIBUTED BEARING ERRORS 

fi DT TIME STEP ( IN HOURS ) 

nERR ACTUAL POSITION ERROR ( TRUE-PREDICTED ) 

A G KALMAN GAIN 

A H MEASUREMENT MATRIX 

fiNTHETAl .NTHETA2 . .NOISY BEARINGS FROM SENSORS 1 AND 2 TO TARGET 

fiNZ MEASURED X ,Y POSITION ( NOISY ) 

a P ERROR COVARIANCE MATRIX 

a PHI STATE TRANSITION MATRIX 

A Q STATE EXCITATION COVARIANCE MATRIX 

a R MEASUREMENT NOISE COVARIANCE MATRIX 

A RNGPCERR PERCENT RANGE ERROR 

rR2D RADIANS TO DEGREES 

a TKM TIME VECTOR ( IN MINUTES ) 

a VX , VY TRUE TARGET VELOCITIES IN X AND Y DIRECTIONS 

a 7X1 ,VY2 SENSOR 1 AND SENSOR 2 VELOCITIES 

a XHAT STATE ESTIMATE 

a XTK , YTK VECTORS OF TRUE TARGET POSITIONS 

*X1K,Y2K VECTORS OF TRUE SENSOR POSITIONS 

A 

A*********************************************************** 

a INITIAL CONDITION INPUTS: 

a 1 INPUT DESIRED RANDOM LINK: ' 

RLSAVE+URL+ 26 5067500 

1 HOW MANY ITERATIONS ARE TO BE RUN (X ) ? ' 

K+Q 

fl| ENTER TRUE TARGET PARAMETERS : XTO , VXF , VXS , YTO , VYF , VYS » 

TRUESTATE+U 

XT 0+T RUE ST AT ELI] 

VX+TRUESTATEl 2 3] 

YTO+TRUESTATELnl 
VY+TRUESTATEZ 5 6] 

• WHEN IS THE TARGET GOING TO TURN ( WHICH ITERATION NO.):' 

TURNED 

' NEXT ENTER SENSOR 1 AND SENSOR 2 POSITION AND VELOCITY ' 

'DATA AS X10 ,7X1 ,Y20 ,VY2 ' 

SENSPOS + □ 

X10+SENSPOSZ11 
VXl-*-SENSPOS [2] 

Y 20+SENSPOS [ 3 ] 

VY2+SENSPOSW 

A 

TIME STEP TO BE USED (DT , IN SECONDS ) ? ' 

DT+U 
DTS+DT 
DT+DTi 3600 

a 1 NO . OF POINTS TO INCLUDE IN THE REGRESSION : ' 
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[61] aW?P«-Q 

[62] P 

[63] ' NOW ENTER ALL A PRIORI ESTIMATES AND MATRICES : ' 

[64] ' ' 

r 5 5 ] q 

[66] ' INITIAL GUESS XU AT ( FOUR ELEMENTS MUST BE ENTERED ) : ' 

[ 67 ] Y.UAT+V 

[58] XHA1+K l oXHAT 

[69] APXHAT+XHAT 

[70] ' ' 

[71] a 

[72] ' INITIAL P MATRIX DIAGONAL ELEMENTS ( FOUR ENTRIES ) : ' 

[73] DIAGONALS 

[ 74 ] p«-( (i4 )° . = i4)x u 4 ^DIAGONAL 

[75] 1 ' 

[76] b 

[77] o' ENTER OFF-DIAGONAL POSITIONAL AND VELOCITY COVARIANCES (2 INPUTS ) : 
[73] OFFDIAG «• 0 0 

[79] P[ 1 ; 3 ] -*-PC 3 ; 1] +OFFDIAG11] 

[80] P [ 2 : 4 ] <-P [ 4 ; 2 ] +OFFDIAG [ 2 ] 

[81] PSAVE+P 

[82] fl 

[33] 1 INITIAL R MATRIX ( 4 NUMBERS IN THE ORDER UL ,UR ,LL ,LR) : ' 

[84] p«-G 



[85] 

[36] 

[87] 

[38] 

[89] 

[SC] 

[31] 

[92] 

[33] 

[ 34 ] 

[95] 

[36] 

[37] 

[98] 

[99] 



RSAVE+R+ 2 2 pR 

b SET UP SOME VECTORS NEEDED TO GENERATE THE Q MATRIX . 
flO( (ol l* (4X360Q ) 1*2 
aPl<- 12 1224 2 4 

aFl+Fl , -PI 

bP 2-«- 4 4 2 2 4 4 , 10p2 

flP3f(8o4 ) ,8*F2 

aF4f- 43433232 

aF4«-F4 ,F4 

a 

t i 

' ENTER MAX BEARING ERROR IN DEG. FOR THETAl AND THETA 2 (£2 INPUTS ) : ' 
B 6 



- 100 - 

[ 101 ] 

[ 102 ] 

[103] 

[104] 

[105] 

[106] 

[107] 

[108] 
[103] 
[110] 
[ 111 ] 
[ 112 ] 

[113] 

[114] 

[115] 

[116] 

[117] 

[118] 
[113] 
[ 120 ] 
[ 121 ] 
[ 122 ] 

[123] 

[124] 

[125] 

[126] 

[127] 

[128] 

[129] 

[130] 



a ************************************************************* 



a INITIALIZED STORAGE VECTORS FOR GRAPHING PURPOSES ONLY 
a 



IGVSTORE+QGSTORE+QVERRSTORE+QERRSTORE+QRESIDERRSTORE+ ( 2 , P+1 )p 0 
2RNGPCERRSTORE*-QMSA VE «- ( 1 , K + 1 ) p 0 



a THE FOLLOWING OUTER LOOP IS FOR GRAPHING THREE SETS OF SENSOR BEARIN 
a ERRORS ON THE SAME GRAPH . THE TRACKING ALGORITHM IS, IN EFFECT , BE IN 
a RUN THREE TIMES , WITH EACH RUN USING A DIFFERENT PAIR OF SENSOR 
a BEARING ERRORS. 

MAIN LOOP *- 1 

I CP : BRGERR+B 6 [ ( " 1 + 2 t-MAINLOOP ) , 2 x MAINLOOP1 
P*-PSA VE 
R+RSAVE 
XHAT+APXHAT 
A1+BRGERRZ11 
A2+BRGERR 12 ] 

pSET UP Q MATRIX INITIALLY TO BE A 4X4 matrix of zeros . 

Q+ 4 4 pO 



a ************************************************************** 



a THE PURPOSE OF THIS PROGRAM SEGMENT IS TO 

a ESTABLISH TRUE TARGET TRACK ( WHICH INCLUDES TWO LEGS ) AND SET UP 
p SENSOR 1 AND SENSOR 2 TRACKS. NOISE-FREE BEARINGS FROM EACH SENSOR 
a TO THE TARGET ARE COMPUTED. NOISY ZERO -MEAN NORMALLY DISTRIBUTED 
a BEARING ERRORS ARE GENERATED AND THEN ADDED TO THE NOISE -FREE 
p BEARINGS . FROM THE INTERSECTION OF THESE NOISY LOB ' S , THE NOISY 
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:i3 

:i3 

:i3 

:i3 

:i3 

:i3 

:is 

:i3 

: 13 

:i4 

:i4 

:i4 

:i4 

:i4 

:i4 

:i4 

14 

:i4 

14 

15 

15 

15 

15 

15 

15 

15 

15 

15 

15 

16 

16 

16 

16 

16i 

16 

16 i 

16' 

16: 

16' 

17i 

17 

17 

17: 

17' 

17 

17i 

17' 

17: 

17 

18i 

18 

18 

18: 

18' 

18 

18! 

18 

18: 

18 : 

13: 

13 

19: 

19: 

19' 

15 

191 

19' 

19! 

1 9 1 

2 C ! 



a (X , 7 ) POSITION OF THE TARGET IS COMPUTED AND STORED FOR LATER USE 
a IN THE TRACKING FILTER AS THE TARGET ' S ' MEASURED ' POSITION FOR 
a THE KTH TIME ITERATION. 

o SET UP SOME INITIAL CALCULATIONS : 

TK+DTxO , \K 
IKM+TK*60 

a 



XTKF+XTO+VXLllxDTxO. xTURN 

XTKS*( l+XTKFl+VXZllxDTxxiK-TURN) 

XTK*XTKF , XTKS 

XIKF+YZO+VY CllxDTxO , xTURN 

YTKS* ( U-YTKF) + VYZ2]xDTx\ (K-TURN) 

YTK+YTKF ,YTKS 

Q X1K+X10+VX1xTK 

Y2K*Y20+VY2xTK 



p 

R2D*18 OtoI 
LVX*X1K=XTK 
LVY*Y2K-YTK 

THETA1K*( C XlK<XTK)xol ) + ~_3o (YTK* (X1K-XTK+LVX) ) 
THETA2K*((Y2K<YTK)xol )+ 3o iXTK* (Y2K-YTK+LVY ) ) 

THETA 1K+(LVXxO .S xol) + (~LVX)x THETA 1 K 
THETA2K+lLVYxO .Sxoi )+(~LVY)xTHETA2K 

a CONTINUE WITH CALCULATIONS : 

a DATAPOINTS* ( 2 , NRP )p 0 
RNGPCERRSTORE*MSAVE*0 
F.ESIDERRSTORE*NZ* 2 1 oO 

GVSTORE*VERRSTORE*ERRSTORE*BSTORE*GSTORE*NZFIX*(2 ,K+1 )p0 

XH AT STORE* 4 1 pO 
P STORE* 4 4 p 0 

p 

H* 24pl0000010 
PHI * (i4)o.=\4 
PHIZ 1 ; 2 1*DT 
PHIZ3;UJ*DT 
aACCEL*0 ,5xDT*2 

aGAMMA *n 2 pACCEL,Q ,DT , 0,0, ACCEL, 0 ,DT 
I*i\ 4)«.=l4 
P 

qrtrt*rtrt*rt*******rtrt**rtrtrt*rt*rt***rtrt**rtrtrt*rtrt**rtrt*rtrt*rtrt**rt*****rtrt** 

a START MAIN LOOP: 

p 

N* 1 

p 

LOOP : G*P+ . x ( ) + . x ®R+H + . xp+ . x 



p 

a * KALMAN GAIN: » 

GVSTORE Z1 ; N1*GZ2 : 11 
GV STORE [ 2 ; A 7 ] *G C 4 ; 2 ] 

GSTORE [1 :NZ*Gll : 1] 
GSTOREZ2;Nl*GZ3:21 
pG 
p 

p 

P*(I-G+.xH)+.xP 

PSTORE*PSTORE ,[0.5xl+l<W]P 

p 

p ' UPDATED P: ' 
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! 0 1 

! 0 2 

!03 

!04 

! 0 5 

!06 

! 3 7 

:08 

! 0 9 

! 1 0 

! 1 1 

: 1 2 

! 1 3 

14 

: 1 5 

'16 

17 

IS 

IS 

20 

21 

22 

23 

24 

25 

26 

27 

23 

29 

30 

31 

32 

33 

34 . 

35' 

36: 

27 

38: 

ss: 

40: 

41: 

42: 

43 ' 

44; 

4 5 : 

46' 

47: 

48: 

45' 

50: 

5i: 

52: 

53 . 

54 ; 

5 5 . 

5e: 

57: 

53: 

ss: 

so: 

si: 

62: 

63! 

64; 

55: 

66 . 

67' 

ss: 

es: 

7c: 



p p 

Z K* 2 1 oXTElNl , YTKIN1 
p ' TRUE TARGET POSITION ' 
p ZK 

p THE NEXT SECTION GENERATES 2 NORMAL BEARING ERRORS . 
p 

U*{? 2Ql0*10 )*10*1C 
SSQ<-( 2x©£/[l] )*C . 3 
SPR 1 *A 1t{R2Dx1 . 36) 

SPR2*A 2 * l?.2Dxl . 36 ) 

5sr0r?rci ;//j-*-flffl*5P^lxSSQx2o(UC2] x2xol ) 

ESTOREZ2 iNl+BE2*SPR2xSSQxlc(UZ21 x 2 xol ) 

ffZ’ffEZ’Al+Z’ffEZ’AltfCtf] +SE1 
NTUETA2+TRETA2KZtn +be 2 

SAl+loNTHETAl 
CAl*2oNTHETAl 
SA2*loNTHETA2 
CA2*2 oNTHETA 2 
I>^2 o {NTH ETA 1 +NTHETA2 ) 

NZFIX C 1 ; AH <-A/Z [ 1 ; ] ■*• ( (Y2KZN1 *SA 2 *CA1 ) - {X1KZN1 *SAl*SA2 ) H D 
NZFIXZ2 :N1*NZZ2:1*{ {XlKZNl xSA1xCA2)-(Y2KZNJxSA2xSA1)Ud 
a NOISE ZliNl *XTK iNl -NZFIX [ 1 ; N1 
p NOISE [ 2 ; N1 +YTK IN ] -NZFIX [ 2 ; N1 
p • TRUE TGT X , Y POSIT MINUS NOISY X , Y POSIT ' 
p NOISE l iNl 

p 

p 

RESIDERR+NZ -H+ . XXHAT 
XHAT*XHAT+G+ . xRESIDERR 

A 

XHATSTORE+XHATSTORE , XHAT 

RESIDERRSTORE+RESIDERRSTORE , RESIDERR 

p DATAPOINTS+DATAPOINTS ,NZ 
oDATAPOINTS+DATAPOINTS , 10 10 -f-XHAT 

aDATAUSE * C 2 . -NRP) + DATAPOINTS 
aSUMX*+/DATAUSEZHl 
qSUMY*+ /DATA USE [ 2 ; ] 
fiSUMX2*+/DATAUSEZl i ] *2 
aSUMXY*+/DATAUSEZl i J *DATAUSEZ 2 ; ] 

^DENOMINATOR* (NRP*SUMX2 ) -SUMX* 2 
p M* ( ( NRP*SUMXY)-SUMXxSUMY)iDENOMINATOR 
QMDEG+R2D* (ol )+ 3 oM 
p MSA VE+MSA VE ,MDEG 

VERRSTOREZl :Nl*VXZl+N>TURN+ll -XHATZ2 ; ] 

VERRSTOREL2 iNl*VYll+N>TURN+ll -XHATl 4;] 

P.TRUE*(+/ZK*ZK)*0 . 5 
AX*- 10 10 /X/MT 
fltf/12V(+/flXx,PX)*0.5 
RNGPCERR * 10 0 x ( iRTRUE ) x ! RTRUE- RHAT 
RNGPCERRSTORE*RNGPCERRSTORE , RNGPCERR 
a 

p • UPDATED XHAT: ' 
p xa^z 1 

ERR*ZK- 2 1 o,XHATLl 3 ;] 
p ' ERROR VECTOR : ' 

ERRSTORE Zl;Nl *ERR Cl;] 

E RESTORE C 2 [ 2 ; ] 

p£7?fl 

p 
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[271] 

[272] 

[273] 

[274] 

[275] 

[276] 

[277] 

[278] 

[279] 

[280] 
[281] 
[282] 

[283] 

[284] 

[285] 

[286] 

[287] 

[288] 

[289] 

[290] 

[291] 

[292] 

[293] 

[294] 

[295] 

[296] 
[297" 
[ 298 ^ 
[299] 
[300" 
[301 
[302 
[303 
[304 
[305 
[306 
[307 
[308 
[309. 

[310] 

[311] 

[312] 
[313" 
[314 
[315. 
[3161 

[317] 

[318] 

[319] 
[320; 
[32i; 
[322: 
[323: 
[32li: 
[325; 
[326; 
[327; 



*P+Q+PH 1+ . xP+ . J 



p 

p ' PREDICTED P : » 
p P 

p 

*XHAT*PHI+ .*XHAT 



p 

p ' PREDICTED XHAT: ' 
p XHAT 

&X2+XHATZ2 ; ] 
fiX4<-mr[4i] 

pXA-«-X4 ,X4,X2,X2, X4 , X4 ,X2 ,X2 ,X2 ,X2 ,X2 ,X2 ,X2 ,X2 ,X2 ,X2 
pXB«-X4 ,X4 ,X4 ,X4 ,X4,X4 ,X4,X4,X4,X4,X2 ,X2 ,X4 ,X4,X2 ,X2 
aQTERMS<rCxFl*XAxXBxDTS*F^ 
aQ* 4 4 oQTERMS 

p CALCULATE TERMS IN THE R MATRIX : 

0 Rl<r(XHATZ3 ;]*2 ) + (XHATLl : ] -X1KLN1 )*2 
SIG12+R1 x ( 3 oAl*R2Dxl . 96 )*2 
P.2*(XHATZl ;]*2 )+ (XHATZ3 : ] -Y2KZN1 )*2 
SIG2 2*R2* ( 3 oA2tR2D*1 . 96 )*2 



Dl+D*2 

UL*( (SIG12*SA2*2 )+ (SIG22xCAl*2) )*D 1 
LR*{ (SIG12*CA2*2)+ (SIG22*SAl*2 ) )*D 1 
CROSS*- ( (SIG12*SA2*CA2 )+ (SIG22xSAl*CAl ) )*D 1 
R* 2 2 p UL .CROSS , CROSS ,LR 
p 'SMA^IX: ' 
p S 
p ' » 

P T **************************************** 

p » ITERATION NO. ' ,9 (N+l) 

p » ' 

N+N+l 

*(N<K+ 1 )/LOOP 



p 

p********************************************************* 

°LQM*0 . 5*1 + 1<MAINL00P 
QGV STORE+QGV STORE , ZLQM1 GV STORE 
QGSTORE+QGSTORE ,ZLQM J GSTORE 
QVERRSTORE+QVERRSTORE . ZLQM1 VERRSTORE 
QERRSTORE+QERRSTORE .ZLQMl ERRSTORE 
QRESIDERRSTORE+QRESIDERRSTORE.ZLQMl RESIDERRSTORE 
RNGPCERRSTORE * ( 1 , oRNGPCERRSTORE )oRNGPCERRSTORE 
QRNGPCERRSTORE+QRNGPCERRSTORE , ZLQH1 RNGPCERRSTORE 
p MSA VE * (l.p MSA VE) p MSA 7S 
p QMSAVE+QMSAVE ,ZLQM1 MSAVE 
MAINLOOP+MAINLOOP+1 
ORL+RLSA VE 

-* (MAINLOOPZO . 5xpB6 )/TOP 
» COMPLETE . • 
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